clear;clc;                                                                 %清除变量 
delete(instrfindall)
global p1 p2 p3;
global P;
global r1 r2 r3;
x3=0;
y3=600;
z3=0;  %作为顶点（机头）坐标
p1=[x3-650 y3-600 z3];
p2=[x3+650 y3-600 z3];
p3=[x3 y3 z3];          %p3是机头，p1是左翼，p2是右翼
%
%% 创建串口
g=serial('com8');                                                          %创建串口对象
set(g,'BaudRate',115200,'StopBits',1,'Parity','none','DataBits',8,'InputBufferSize',8000);
fopen(g);

%% 读取串口数据
P = [10,10,10];
T = [10,10,5];
for i=1:1:10
    D=[];
    k=0;
while k<=200
a = fscanf(g,'%s',1000);
D =[D,a];           %读取并连接字符串
k=k+1;
end
%disp(D);
disp('Another Time');
[r1,r2,r3]=dispose(D);   %分析数据得到距离信息
%RESULT=measure_result();

%% 求解空间点坐标
%P0 =[100,100,100];
P0 = P;
options=optimset('MaxFunEvals',1e20,'TolFun',1e-18,'display','off');
P = fsolve(@root_p,P0,options)
%disp(P);
hold on;
plot3(P(1),P(2),P(3),'o');
%axis(-10000,10000,-10000,10000,-5000,5000);

%% 求解空间点在平面上的投影点
fun2 = @root_t;
T0 = T;
options=optimset('MaxFunEvals',1e15,'TolFun',1e-18);
T = fsolve(fun2,T0,options)
%disp(T);

%% 显示结果
RESULT=[Ver_theta_d Fla_theta_d];
disp(RESULT);
end

fclose(g);                                                                 %关闭串口
%}



